package org.example.tree;

import java.util.*;

/**
 * @ClassName AStarPathfinding
 * @Description TODO
 * @Author hongjun
 * @Date 2024/5/17 下午11:38
 * @Version 1.0
 */
public class AStarPathfinding {
    static class Node implements Comparable<Node> {
        int x, y;
        double g; // 从起点到当前节点的实际代价
        double h; // 从当前节点到目标的启发式代价（估计）
        double f; // f = g + h
        Node parent;

        public Node(int x, int y) {
            this.x = x;
            this.y = y;
        }

        @Override
        public int compareTo(Node other) {
            return Double.compare(this.f, other.f);
        }
    }

    public  static  List<Node> findPath(Node start, Node goal,Quadtree quadtree) {
        PriorityQueue<Node> openList = new PriorityQueue<>();
        Set<Node> closedList = new HashSet<>();

        start.g = 0;
        start.h = heuristic(start, goal);
        start.f = start.g + start.h;
        openList.add(start);

        while (!openList.isEmpty()) {
            Node current = openList.poll();

            if (current.equals(goal)) {
                return reconstructPath(current);
            }

            closedList.add(current);

            List<Node> neighbors = getNeighbors(current);
            for (Node neighbor : neighbors) {
                if (closedList.contains(neighbor)) {
                    continue;
                }

                if (quadtree.isBlocked(neighbor.x, neighbor.y)) { // 使用Quadtree进行碰撞检测
                    continue;
                }

                double tentativeGScore = current.g + distBetween(current, neighbor);

                if (openList.isEmpty() || tentativeGScore < neighbor.g) {
                    neighbor.g = tentativeGScore;
                    neighbor.h = heuristic(neighbor, goal);
                    neighbor.f = neighbor.g + neighbor.h;
                    neighbor.parent = current;

                    if (openList.isEmpty() || neighbor.compareTo(openList.peek()) < 0) {
                        openList.add(neighbor);
                    }
                }
            }
        }

        return null; // 没有找到路径
    }

    private static List<Node> reconstructPath(Node current) {
        List<Node> path = new ArrayList<>();
        while (current != null) {
            path.add(current);
            current = current.parent;
        }
        Collections.reverse(path); // 因为我们从目标回溯到起点，所以需要反转列表
        return path;
    }

    private static List<Node> getNeighbors(Node node) {
        List<Node> neighbors = new ArrayList<>();
        // 添加上下左右四个方向的邻居节点（假设地图是网格状的）
        for (int dx = -1; dx <= 1; dx++) {
            for (int dy = -1; dy <= 1; dy++) {
                if (dx == 0 && dy == 0) continue; // 跳过当前节点
                int newX = node.x + dx;
                int newY = node.y + dy;
                // 在这里可以添加额外的逻辑来确保新坐标在地图范围内
                neighbors.add(new Node(newX, newY));
            }
        }
        return neighbors;
    }

    private static double heuristic(Node a, Node b) {
        // 使用曼哈顿距离作为启发式函数
        return Math.abs(a.x - b.x) + Math.abs(a.y - b.y);
    }

    private static double distBetween(Node a, Node b) {
        // 在网格地图上，距离通常是曼哈顿距离
        return Math.abs(a.x - b.x) + Math.abs(a.y - b.y);
    }
}
